/*Names :D
 * Eugene Che
 * David Sparkman
 * BENJYYYYYYYYYYYYY
 * Alex Renda 
 * Andrew Chang
 */

/* Colby's name
 * Colby Guan
 */

package org._649mset.omnibot;

import edu.wpi.first.wpilibj.*;

public class MainBot extends SimpleRobot {
	
	private final Joystick mXbox;
    private final HolonomicDT mDT;
    
    public MainBot() {
        mXbox = new Joystick(1);
        mDT = new HolonomicDT(new int[] {1,2,3,4});
        getWatchdog().setEnabled(true);
        getWatchdog().setExpiration(.5);
    }
	
    public void autonomous() {
        while(isAutonomous() && isEnabled()) {
            getWatchdog().feed();
        }
    }
    public void operatorControl() {
        while(isOperatorControl() && isEnabled()) {
            getWatchdog().feed();
            mDT.drive(fixInput(mXbox.getY()),fixInput(mXbox.getX()),fixInput(mXbox.getTwist()));
        }
    }
    
    public double fixInput(double input) {
        if(Math.abs(input) < 0.15)
            return 0;
        return input;
    }
}
